#include "io_1061s.h"

#define IS_VALID_ID(board_id)               (board_id < dev_nums && io_1061s_list[board_id].is_init && io_1061s_list[board_id].is_online)
#define IS_VALID_DEV_ID(board_id, dev_id)   (dev_id < register_dev_nums[board_id])

enum{
    STEP_SET_IO_OUTPUT = 0,
    STEP_SET_DEFAULT_OUT,
    STEP_SET_OVER_TIME,
    STEP_GET_CURR_OUTPUT,
    STEP_GET_OUTPUT,
    STEP_GET_PULSE,
    STEP_SET_PULSE,
    STEP_SET_DEV_MOVE_TIME,
    STEP_SET_DEV_MOVE_POS,
    STEP_GET_ENCODER,
    STEP_CLEAR_ENCODER,

    STEP_MAX,
};

// 相关定义
/******************************************************************/
static io_1061s_t *io_1061s_list = NULL;
static uint8_t dev_nums = 0;                //子板数量
static uint8_t register_dev_nums[6] = {0};  //子板注册的设备数量（最多支持6块子板，可增加）
static bool isSetMoveTime = false;
static bool isSetMovePos = false;
static bool isGetMoveEncoder = false;




// 内部函数声明
/******************************************************************/
static void _io_1061s_loop_thread(void *arg);
static int _io_1061s_get_index_by_node_id(uint32_t node_id);




// 外部接口函数
/******************************************************************/
/**
 * @brief       io 子板驱动初始化
 * @param[in]	pdev                子板列表（驱动中会将 id 变换成符合 canopen 的标准，此列表需静态，驱动不会拷贝）
 * @param[in]	list_size           id 列表长度
 * @param[in]	callback            io 子板事件回调函数
 * @return      bool                执行结果
 */
bool io_1061s_init(io_1061s_t *const pdev, uint8_t list_size, event_callback_t callback)
{
    int ret;
    if (NULL == pdev || 0 == list_size || NULL != io_1061s_list){
        printf("ERROR: Params error\r\n");
        return false;
    }
    io_1061s_list = pdev;
    dev_nums = list_size;

    for (int i = 0; i < dev_nums; i++) {
        io_1061s_list[i].over_time = DEFAULT_COMM_OVERTIME_MS;
		io_1061s_list[i].is_online = false;
        io_1061s_list[i].callback = callback;
        io_1061s_list[i].output = io_1061s_list[i].default_out;
    }

    // 初始化 can_controller
    CHECK_ERR(can_controller_init(), "can_controller_init failed", err_1);

    aos_msleep(20);
    // 注册接收节点
    io_1061s_list[0].rx_queue = can_controller_add_node(0, 0x580 + io_1061s_list[0].node_id, 10);
    CHECK_NULL(io_1061s_list[0].rx_queue, "add node failed", err_2);
    io_1061s_list[0].is_init = true;

    // 多个设备时追加注册接收节点，数据接收队列使用同一个
    for (int i = 1; i < dev_nums; i++) {
        CHECK_ERR(can_controller_add_node_static(0, 0x580 + io_1061s_list[i].node_id, io_1061s_list[0].rx_queue), "add nodes failed", err_3);
        io_1061s_list[i].rx_queue = io_1061s_list[0].rx_queue;
        io_1061s_list[i].is_init = true;
    }

    // 创建轮询线程
    CHECK_NZERO(aos_task_new("_io_1061s_loop_thread", _io_1061s_loop_thread, NULL, 8192), "creat thread failed", err_3);

    printf("IO board init success\r\n");
    return true;

err_3:
    for (uint8_t i = 0; i < dev_nums; i++) {
        can_controller_del_node_byId(0, 0x580 + io_1061s_list[i].node_id);
    }
err_2:
    io_1061s_list = NULL;
err_1:
    return false;
}
/**
 * @brief       设置 io 子板输出状态
 * @param[in]	board_id            io 子板索引 id（以 0 开始）
 * @param[in]	state               输出状态，以 bit 位代表每个 io 的电平
 * @return      int                 0：成功， -1：失败
 */
int io_1061s_set_output_pins(uint8_t board_id, uint32_t state)
{
    if (IS_VALID_ID(board_id)){
        io_1061s_list[board_id].output = state;
        return 0;
    }
    return -1;
}
/**
 * @brief       获取 io 子板输出状态
 * @param[in]	board_id            io 子板索引 id（以 0 开始）
 * @param[out]	pResult             输出结果，以 bit 位代表每个 io 的电平
 * @return      int                 0：成功， -1：失败
 */
int io_1061s_get_output_pins(uint8_t board_id, uint32_t *pResult)
{
    if (IS_VALID_ID(board_id)){
        *pResult = io_1061s_list[board_id].output;
        return 0;
    }
    return -1;
}
/**
 * @brief       获取 io 子板的真实输出状态
 * @param[in]	board_id            io 子板索引 id（以 0 开始）
 * @param[out]	pResult             输出结果，以 bit 位代表每个 io 的电平
 * @return      int                 0：成功， -1：失败
 */
int io_1061s_get_output_pins_real(uint8_t board_id, uint32_t *pResult)
{
    if (IS_VALID_ID(board_id)){
        io_1061s_list[board_id].step = STEP_GET_CURR_OUTPUT;
        io_1061s_list[board_id].ack_get_output = false;
        uint16_t waitTimes = dev_nums * 50 + 10;
        while(!(io_1061s_list[board_id].ack_get_output) && waitTimes--){
            aos_msleep(3);
        }
        if (waitTimes){
            *pResult = io_1061s_list[board_id].outputReal;
            return 0;
        }
    }
    return -1;
}
/**
 * @brief       获取 io 子板输入状态
 * @param[in]	board_id            io 子板索引 id（以 0 开始）
 * @param[in]	pResult             输出结果，以 bit 位代表每个 io 的电平
 * @return      int                 0：成功， -1：失败
 */
int io_1061s_get_input_pins(uint8_t board_id, uint32_t *pResult)
{
    if (IS_VALID_ID(board_id)){
        *pResult = io_1061s_list[board_id].input;
        return 0;
    }
    return -1;
}
/**
 * @brief       设置 io 子板默认输出状态(注意初始化后不可紧接着设置，此时可能还不在线)
 * @param[in]	board_id            io 子板索引 id（以 0 开始）
 * @param[in]	state               默认输出状态，以 bit 位代表每个 io 的电平
 * @return      int                 0：成功， -1：失败
 */
int io_1061s_set_default_output(uint8_t board_id, uint32_t state)
{
    if (IS_VALID_ID(board_id)){
        io_1061s_list[board_id].step = STEP_SET_DEFAULT_OUT;
        io_1061s_list[board_id].ack_default_out = false;
        io_1061s_list[board_id].default_out = state;
        uint16_t waitTimes = 30;
        while(!(io_1061s_list[board_id].ack_default_out) && waitTimes--){
            aos_msleep(3);
        }
        return io_1061s_list[board_id].ack_default_out ? 0 : -1;
    }
    return -1;
}
/**
 * @brief       设置 io 子板超时时间(注意初始化后不可紧接着设置，此时可能还不在线)
 * @param[in]	board_id            io 子板索引 id（以 0 开始）
 * @param[in]	time_ms             超时时间（ms，超过此时间，IO 子板将会重启）
 * @return      int                 0：成功， -1：失败
 */
int io_1061s_set_over_time(uint8_t board_id, uint32_t time_ms)
{
    if (IS_VALID_ID(board_id)){
        io_1061s_list[board_id].step = STEP_SET_OVER_TIME;
        io_1061s_list[board_id].ack_over_time = false;
        io_1061s_list[board_id].over_time = time_ms;
        uint16_t waitTimes = 30;
        while(!(io_1061s_list[board_id].ack_over_time) && waitTimes--){
            aos_msleep(3);
        }
        return io_1061s_list[board_id].ack_over_time ? 0 : -1;
    }
    return -1;
}
/**
 * @brief       获取指定板id的指定pinId的脉冲数
 * @param[in]	board_id            
 * @param[in]	pin_id              
 * @param[in]	pResult             
 * @return      int                 
 */
int io_1061s_get_pulse(uint8_t board_id, uint8_t pin_id, uint32_t * const pResult)
{
    if (IS_VALID_ID(board_id)){
        io_1061s_list[board_id].step = STEP_GET_PULSE;
        io_1061s_list[board_id].ack_get_pulse = false;
        io_1061s_list[board_id].get_pulse_pin_id = pin_id;
        uint16_t waitTimes = dev_nums * 50 + 10;
        while(!(io_1061s_list[board_id].ack_get_pulse) && waitTimes--){
            aos_msleep(3);
        }
        if (waitTimes && io_1061s_list[board_id].get_pulse_pin_id == pin_id){
            *pResult = io_1061s_list[board_id].pulse_result;
            return 0;
        }
    }
    return -1;
}

/**
 * @brief       设置指定板id的指定io为记脉冲模式
 * @param[in]	board_id            
 * @param[in]	pin_bit_id          需要设置记脉冲模式的io，相应比特位置1
 * @return      int                 
 */
/* int io_1061s_set_pulse(uint8_t board_id, uint32_t pin_bit_id, SUB_INDEX_CMD_t index)
{
    if (IS_VALID_ID(board_id)){
        io_1061s_list[board_id].step = STEP_SET_PULSE;
        io_1061s_list[board_id].ack_set_pulse = false;
        io_1061s_list[board_id].set_pulse_pin_bit_id = pin_bit_id;
        io_1061s_list[board_id].sub_index = index;
        uint16_t waitTimes = dev_nums * 50 + 10;
        while(!(io_1061s_list[board_id].ack_set_pulse) && waitTimes--){
            aos_msleep(3);
        }
        return io_1061s_list[board_id].ack_over_time ? 0 : -1;
    }
    return -1;
} */

/**
 * @brief       清除指定板id的指定io脉冲计数值
 * @param[in]	board_id            
 * @param[in]	pin_id              
 * @return      int                 
 */
int io_1061s_clear_pulse(uint8_t board_id, uint32_t pin_id)
{
    if (IS_VALID_ID(board_id)){
        io_1061s_list[board_id].step = STEP_SET_PULSE;
        io_1061s_list[board_id].ack_set_pulse = false;
        io_1061s_list[board_id].set_pulse_pin_id = pin_id;
        // io_1061s_list[board_id].sub_index = index;
        uint16_t waitTimes = dev_nums * 50 + 10;
        while(!(io_1061s_list[board_id].ack_set_pulse) && waitTimes--){
            aos_msleep(3);
        }
        return io_1061s_list[board_id].ack_over_time ? 0 : -1;
    }
    return -1;
}

/**
 * @brief       设置管理设备的移动时间
 * @param[in]	board_id            子板id
 * @param[in]	dev_id              注册的设备id
 * @param[in]	dir                 移动方向（>0为正向，=0停止，<0反向）
 * @param[in]	time_ms             移动时间
 * @return      int                 
 */
int io_1061s_set_dev_move_time(uint8_t board_id, uint8_t dev_id, int8_t dir, uint32_t time_ms)
{
    isSetMoveTime = true;
    if (IS_VALID_ID(board_id) && IS_VALID_DEV_ID(board_id, dev_id)){
        io_1061s_list[board_id].step = STEP_SET_DEV_MOVE_TIME;
        io_1061s_list[board_id].ack_set_move_time = false;
        io_1061s_list[board_id].dev_id = dev_id;
        if(time_ms > 0x00FFFFFF){                           //通讯数据高4位为dev_id,高4~8位为dir，剩余位数作为时间数据
            isSetMoveTime = false;
            return -2;
        }
        io_1061s_list[board_id].move_time = time_ms;
        if(dir > 0){
            io_1061s_list[board_id].direction = 0xA;        //该数据结构只占4比特，方向值小于0xF
        }
        else if(dir < 0){
            io_1061s_list[board_id].direction = 0x5;
        }
        else{
            io_1061s_list[board_id].direction = 0;
        }
        
        uint16_t waitTimes = dev_nums * 50 + 10;
        while(!(io_1061s_list[board_id].ack_set_move_time) && waitTimes--){
            aos_msleep(3);
        }
        isSetMoveTime = false;
        return io_1061s_list[board_id].ack_set_move_time ? 0 : -1;
    }
    isSetMoveTime = false;
    return -3;
}

/**
 * @brief       设置管理设备的移动位置
 * @param[in]	board_id            子板id
 * @param[in]	dev_id              注册的设备id
 * @param[in]	move_pos            移动位置
 * @return      int                 
 */
int io_1061s_set_dev_move_pos(uint8_t board_id, uint8_t dev_id, int32_t move_pos)
{
    isSetMovePos = true;
    if (IS_VALID_ID(board_id) && IS_VALID_DEV_ID(board_id, dev_id)){
        io_1061s_list[board_id].step = STEP_SET_DEV_MOVE_POS;
        io_1061s_list[board_id].ack_set_move_pos = false;
        io_1061s_list[board_id].dev_id = dev_id;
        if(abs(move_pos) > 0x00FFFFFF){                           //通讯数据高4位为dev_id,高4~8位为位置坐标正负，剩余位数作为位置数据
            isSetMovePos = false;
            return -2;
        }
        else{
            if(move_pos < 0){
                io_1061s_list[board_id].move_pos = (uint32_t)((0x05 << 24) | abs(move_pos));
            }
            else{
                io_1061s_list[board_id].move_pos = (uint32_t)abs(move_pos);
            }
        }
        
        uint16_t waitTimes = dev_nums * 50 + 10;
        while(!(io_1061s_list[board_id].ack_set_move_pos) && waitTimes--){
            aos_msleep(3);
        }
        isSetMovePos = false;
        return io_1061s_list[board_id].ack_set_move_pos ? 0 : -1;
    }
    isSetMovePos = false;
    return -3;
}

/**
 * @brief       获取管理设备的编码器值
 * @param[in]	board_id            子板id
 * @param[in]	dev_id              注册的设备id
 * @param[in]	pResult             
 * @return      int                 
 */
int io_1061s_get_dev_encoder(uint8_t board_id, uint8_t dev_id, int32_t * const pResult)
{
    isGetMoveEncoder = true;
    if (IS_VALID_ID(board_id) && IS_VALID_DEV_ID(board_id, dev_id)){
        io_1061s_list[board_id].step = STEP_GET_ENCODER;
        io_1061s_list[board_id].ack_get_encoder = false;
        io_1061s_list[board_id].dev_id = dev_id;

        uint16_t waitTimes = dev_nums * 20 + 10;
        while(!(io_1061s_list[board_id].ack_get_encoder) && waitTimes--){
            aos_msleep(3);
        }
        if (waitTimes && io_1061s_list[board_id].getEncDevId == dev_id){
            *pResult = io_1061s_list[board_id].devEncoder;
            isGetMoveEncoder = false;
            return 0;
        }
    }
    isGetMoveEncoder = false;
    return -1;
}

/**
 * @brief       清除管理设备的编码器值
 * @param[in]	board_id            子板id
 * @param[in]	dev_id              注册的设备id
 * @return      int                 
 */
int io_1061s_clear_encoder(uint8_t board_id, uint8_t dev_id)
{
    if (IS_VALID_ID(board_id) && IS_VALID_DEV_ID(board_id, dev_id)){
        io_1061s_list[board_id].step = STEP_CLEAR_ENCODER;
        io_1061s_list[board_id].ack_clear_encoder = false;
        io_1061s_list[board_id].dev_id = dev_id;

        uint16_t waitTimes = dev_nums * 20 + 10;
        while(!(io_1061s_list[board_id].ack_clear_encoder) && waitTimes--){
            aos_msleep(3);
        }
        return io_1061s_list[board_id].ack_over_time ? 0 : -1;
    }
    return -2;
}

uint32_t txTotalCnts[6] = {0};
uint32_t rxTotalCnts[6] = {0};

void get_can_data_cnt(uint32_t *txCnts, uint32_t *rxCnts, uint8_t boadrdId)
{
    *txCnts = txTotalCnts[boadrdId];
    *rxCnts = rxTotalCnts[boadrdId];
}


// 内部函数
/******************************************************************/
/**
 * @brief       io 子板轮询读取线程
 * @param[in]	arg                 。。。
 */
static void _io_1061s_loop_thread(void *arg)
{
    flexcan_frame_t Frame;
    uint32_t size, ret;
    uint32_t devCrlData = 0;
	
	for (uint8_t i = 0; i < dev_nums; i++){
		io_1061s_list[i].timeStamp = aos_now_ms();
        io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
	}

    for(;;){
        for (uint8_t i = 0; i < dev_nums; i++)
        {
            if(txTotalCnts[i]++ > 0x8FFFFFFF){
                txTotalCnts[i] = txTotalCnts[i] >> 8;
                rxTotalCnts[i] = rxTotalCnts[i] >> 8;
            }
            // printf("===%04X, step %d\r\n",0x600 + io_1061s_list[i].node_id, io_1061s_list[i].step);
            switch(io_1061s_list[i].step){
                case STEP_SET_IO_OUTPUT:
                    // 设置输出状态
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_IO_STATE, 0, io_1061s_list[i].output);
                break;

                case STEP_SET_DEFAULT_OUT:
                    // 设置默认输出状态
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_IO_DEFAULT, 0, io_1061s_list[i].default_out);
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;

                case STEP_SET_OVER_TIME:
                    // 设置超时时间
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_COM_OVERTIME, 0, io_1061s_list[i].over_time);
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;

                case STEP_GET_CURR_OUTPUT:
                    // 读取IO子板当前输出(首次连接时需要读取以初始化本地output)
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_GET_OUTPUT, 0, 0);
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;

                case STEP_GET_PULSE:
                    // 读取IO子板脉冲
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_GET_PULSE, io_1061s_list[i].get_pulse_pin_id, 0);
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;

                case STEP_SET_PULSE:
                    // 设置io为计脉冲模式
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_SET_PULSE, 0, io_1061s_list[i].set_pulse_pin_id);
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;

                case STEP_SET_DEV_MOVE_TIME:
                    // 设置设备移动时间
                    devCrlData = 0;
                    devCrlData = ((uint32_t)io_1061s_list[i].dev_id) << 28;
                    devCrlData |= ((uint32_t)io_1061s_list[i].direction) << 24;
                    devCrlData |= (uint32_t)io_1061s_list[i].move_time;
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_SET_DEV_MOVE_TIME, 0, devCrlData);
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;

                case STEP_SET_DEV_MOVE_POS:
                    // 设置设备移动位置
                    devCrlData = 0;
                    devCrlData = io_1061s_list[i].dev_id << 28;
                    devCrlData |= io_1061s_list[i].move_pos;
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_SET_DEV_MOVE_POS, 0, devCrlData);
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;

                case STEP_GET_ENCODER:
                    // 读取IO子板脉冲
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_GET_ENCODER, io_1061s_list[i].dev_id, 0);
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;

                case STEP_CLEAR_ENCODER:
                    // 清除子板编码器计数值
                    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[i].node_id, 0x23, INDEX_CLEAR_ENCODER, io_1061s_list[i].dev_id, 0);
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;

                default:
                    io_1061s_list[i].step = STEP_SET_IO_OUTPUT;
                break;
            }
            

            if (0 == aos_queue_recv(io_1061s_list[i].rx_queue, 10, &Frame, &size)){
                //printf("Recv Success id:%X data0:%04X  data1:%04X\r\n", Frame.id >> 18, Frame.dataWord0, Frame.dataWord1);
                uint8_t cmd = Frame.dataByte0;
                uint16_t index = Frame.dataByte1 | Frame.dataByte2 << 8;
                uint8_t subindex = Frame.dataByte3;
                uint32_t data = Frame.dataWord1;
                uint32_t node_id = Frame.id >> 18;
                //printf("cmd: %X index: %X subindex: %X dlc: %X\r\n", cmd, index, subindex, Frame.length);
			
				/*绝大多数情况下i与接收id对应,但有些情况下可能当前轮询节点收到其它节点数据情况*/
				int8_t id = _io_1061s_get_index_by_node_id(node_id & 0x7F);
				if (-1 != id){
                    rxTotalCnts[id]++;
					io_1061s_list[id].timeStamp = aos_now_ms();

					switch (index)
					{
						case INDEX_IO_STATE:		// 设置IO回应
							if (data != io_1061s_list[id].input){      // IO 输入状态改变回调
								if (io_1061s_list[id].callback){
									uint32_t event_data[3] = {data, io_1061s_list[id].input, data ^ io_1061s_list[id].input};
									io_1061s_list[id].callback(node_id & 0x7F, EVENT_INPUT_CHANGE, &event_data);
								}
							}
							io_1061s_list[id].input = data;
						break;

						case INDEX_IO_DEFAULT:      // 设置默认IO状态回应
							//配置默认状态成功
							println("board <%d> config default out: 0x%04X OK\r\n", node_id & 0x7F, data);
							io_1061s_list[id].ack_default_out = true;
							io_1061s_list[id].default_out = data;
						break;
						
						case INDEX_COM_OVERTIME:	// 设置超时时间回应
							//设置超时时间成功
							println("board <%d> config over time: %d OK\r\n", node_id & 0x7F, data);
							io_1061s_list[id].over_time = data;
							io_1061s_list[id].ack_over_time = true;
						break;

						case INDEX_GET_OUTPUT:	    // 读取输出状态回应
							//读取输出状态成功
							println("board <%d> curr output: 0x%04X OK\r\n", node_id & 0x7F, data);
							io_1061s_list[id].outputReal = data;
						break;

                        case INDEX_SET_PULSE:	    //设置io口是否为读脉冲数状态
                            io_1061s_list[id].ack_set_pulse = true;
                            io_1061s_list[id].sub_index = subindex;
                            io_1061s_list[id].set_pulse_pin_id = data;
                        break;

                        case INDEX_GET_PULSE:	    //读取脉冲数状态
                            io_1061s_list[id].ack_get_pulse = true;
                            io_1061s_list[id].get_pulse_pin_id = subindex;
                            io_1061s_list[id].pulse_result = data;
                        break;

                        case INDEX_SET_DEV_MOVE_TIME:	    //设置设备移动时间
                            println("board <%d> config dev %d move time: %d OK\r\n", node_id & 0x7F, data >> 28, data & 0x00FFFFFF);
							io_1061s_list[id].move_time = data;
							io_1061s_list[id].ack_set_move_time = true;
                        break;

                        case INDEX_SET_DEV_MOVE_POS:	    //设置设备移动位置
                            println("board <%d> config dev %d move pos: %d OK\r\n", node_id & 0x7F, data >> 28, data & 0x00FFFFFF);
							io_1061s_list[id].move_pos = data;
							io_1061s_list[id].ack_set_move_pos = true;
                        break;

                        case INDEX_GET_ENCODER:	            //读取编码器值
                            io_1061s_list[id].ack_get_encoder = true;
                            io_1061s_list[id].getEncDevId = subindex;
                            if((data & 0xFF000000) > 0){  //高8位为正负号识别码
                                io_1061s_list[id].devEncoder = 0 - (int32_t)(data & 0x00FFFFFF);
                            }
                            else{
                                io_1061s_list[id].devEncoder = data & 0x00FFFFFF;
                            }
                            // println("board <%d> dev %d encoder: %d\r\n", node_id & 0x7F, subindex, io_1061s_list[id].devEncoder);
                        break;

                        case INDEX_CLEAR_ENCODER:	    //清除编码器值
                            io_1061s_list[id].ack_clear_encoder = true;
                            io_1061s_list[id].sub_index = subindex;
                        break;
					
					default:
						break;
					}
				}
			}            
            if (aos_now_ms() - io_1061s_list[i].timeStamp > io_1061s_list[i].over_time){
                if (io_1061s_list[i].is_online){
                    io_1061s_list[i].is_online = false;
					if (io_1061s_list[i].callback){
                        uint32_t event_data = io_1061s_list[i].is_online;
						io_1061s_list[i].callback(io_1061s_list[i].node_id, EVENT_LINK, &event_data);
					}
                }
            }else{
				if (!io_1061s_list[i].is_online){
                    io_1061s_list[i].is_online = true;
					if (io_1061s_list[i].callback){
                        uint32_t event_data = io_1061s_list[i].is_online;
						io_1061s_list[i].callback(io_1061s_list[i].node_id, EVENT_LINK, &event_data);
					}
                }
			}
        }
		aos_msleep(10);
    }
}
/**
 * @brief       通过节点 node_id 获取索引 id
 * @param[in]	node_id             节点 id （实际的 can id）
 * @return      int                 索引 id，-1：失败
 */
static int _io_1061s_get_index_by_node_id(uint32_t node_id){
    for (uint8_t i = 0; i < dev_nums; i++) {
        if (node_id == io_1061s_list[i].node_id){
            return i;
        }
    }
    return -1;
}

/**
 * @brief       配置控制设备的IO
 * @param[in]	board_id            子板id
 * @param[in]	dev_id              注册的设备id
 * @param[in]	data                配置数据
 * @return      int                 
 */
int _io_1061s_config_dev_crl_pin(uint8_t board_id, uint8_t dev_id, uint32_t data){
    uint32_t configPulseZeroData = 0;
    uint32_t configDevDriverData = 0;

    register_dev_nums[board_id]++;
    aos_msleep(10); 
    //INDEX_SET_ZERO 数据：
    //低8位:           零点传感器引脚
    //低8位~低16位:    脉冲计数引脚
    //低16位~低28位:   预留
    //高4位:           配置的设备索引号
    configPulseZeroData = (uint32_t)(dev_id) << 28;
    configPulseZeroData |= (data >> 16);
    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[board_id].node_id, 0x23, INDEX_SET_PULSE_ZERO_PIN, 0, configPulseZeroData);
    aos_msleep(10);                                                  //can非阻塞式发送，两条消息之间必须要加点延时才能发成功
    //INDEX_SET_DEV_DRIVER_PIN 数据：
    //低8位:           正向控制引脚
    //低8位~低16位:    反向控制引脚
    //低16位~低24位:   预留
    //高4位:           配置的设备索引号
    configDevDriverData = (uint32_t)(dev_id) << 28;
    configDevDriverData |= (data & 0x0000FFFF);
    can_control_send_sdo_frame(0, 0x600 + io_1061s_list[board_id].node_id, 0x23, INDEX_SET_DEV_DRIVER_PIN, 0, configDevDriverData);
    aos_msleep(10);
}

bool get_set_move_time_flag(void)
{
    return isSetMoveTime;
}

bool get_set_move_pos_flag(void)
{
    return isSetMovePos;
}

bool get_encoder_flag(void)
{
    return isGetMoveEncoder;
}
